#include "FOC.h"
#include "math.h"
#include "dma.h"
#include "usart.h"
#include "i2c.h"
#include "AS5600.h"
#include "tim.h"
#include "PID.h"

#define PI 3.1415926535f
#define _3PI_2 4.71238898f
#define Pole_Pairs 7  //电机极对数  7
#define DIR -1


uint16_t Real_Angle;
float Real_raw;
float Zero_electric_angle = 0.0;
float Voltage_limit = 12.6, voltage_power_supply = 12.6;
float Ua = 0,Ub = 0,Uc = 0;
float dc_a = 0,dc_b = 0,dc_c = 0;
float Ualpha = 0,Ubeta = 0;

float shaft_angle = 0.0f;

float Last_Error = 0.0;
float Last_intergration = 0.0;
// float last_angle = 0.0;
// float full_rotations = 0.0;

float Target_Location = 0.0f;//串口输入的目标角度（浮点数）
volatile float Target_Speed = 0.0f;


void FOC_Init(void)
{
    Set_Phase_Voltage(3, 0, _3PI_2);
    HAL_Delay(1000);
    Zero_electric_angle = Get_Electric_Angle();
    Set_Phase_Voltage(0, 0, _3PI_2);
    HAL_Delay(1000);
    // DMA_Printf("%f\r\n",Zero_electric_angle);
}

//limit value
float Constrain(float amt, float low, float high)
{
    return ((amt<low)?(low):((amt)>(high)?(high):(amt)));
}

//turn angle to 0~2PI
float Angle_Constrain(float angle)
{
   float a = fmod(angle, 2*PI);
   return ((a >= 0) ? a : (a + 2 * PI));
}

//turn angle to electric angle
float Get_Electric_Angle()
{
    // return Angle_Constrain(Get_AS5600_Angle() * Pole_Pairs * DIR) - Zero_electric_angle;
    return (Angle_Constrain(Get_AS5600_Angle() * (float)Pole_Pairs * (float)DIR) - Zero_electric_angle);
}

void Set_PWM(float Ua, float Ub, float Uc)
{
    float U_a = 0.0;
    float U_b = 0.0;
    float U_c = 0.0;

    U_a = Constrain(Ua, 0.0f, Voltage_limit);
    U_b = Constrain(Ub, 0.0f, Voltage_limit);
    U_c = Constrain(Uc, 0.0f, Voltage_limit);

    dc_a = Constrain(U_a / voltage_power_supply, 0.0f, 1.0f) * 2800.0f;
    dc_b = Constrain(U_b / voltage_power_supply, 0.0f, 1.0f) * 2800.0f;
    dc_c = Constrain(U_c / voltage_power_supply, 0.0f, 1.0f) * 2800.0f;

    // DMA_Printf("%f,%f,%f\r\n", dc_a, dc_b, dc_c);

    // Send_Data[0] = dc_a;
    // Send_Data[1] = dc_b;
    // Send_Data[2] = dc_c;
    // Print_Vofa_JustFloat(Send_Data, 3);


    PWM_OUT(dc_a, dc_b, dc_c);
}

//FOC核心算法，克拉克逆变换和帕克逆变换
float test_angle = 0.0, last_test_angle = 0.0;
void Set_Phase_Voltage(float Uq, float Ud, float angle_el)
{
    angle_el = Angle_Constrain(angle_el);

    //帕克逆变换
    Ualpha = -Uq * sin(angle_el);
    Ubeta  = Uq * cos(angle_el);

    //克拉克逆变换
    Ua = Ualpha + voltage_power_supply / 2;
    Ub = (sqrt(3) * Ubeta - Ualpha) / 2 + voltage_power_supply / 2;
    Uc = -(Ualpha + sqrt(3) * Ubeta) / 2 + voltage_power_supply / 2;

    Set_PWM(Ua, Ub, Uc);

    // DMA_Printf("%f,%f,%f\r\n", Ua, Ub, Uc);

    
}

// float Get_AS5600_Angle(void)
// {
//     float jiaodu = 0;
//     float hudu = 0;
//     uint8_t IIC_buffer[2] = {0};
    // HAL_I2C_Mem_Read(&hi2c1, 0x6c, 0x0c, I2C_MEMADD_SIZE_8BIT, IIC_buffer, 2, 100);

//     jiaodu = (IIC_buffer[0] * 256 + IIC_buffer[1])*0.087890625;
//     hudu = (IIC_buffer[0] * 256 + IIC_buffer[1])*(2.0f*PI) / 4096;

//     // return jiaodu;
//     return hudu;
// }

// float Get_Sum_Angle()
// {
//     float this_angle = Get_AS5600_Angle();
//     float D_angle = 0.0;
//     D_angle = this_angle - last_angle;
//     if(D_angle > 5.2f)
//     {
//         full_rotations -= 1; 
//     }
//     if(D_angle < -5.2f)
//     {
//         full_rotations += 1;
//     }
//     last_angle = this_angle;
//     return (full_rotations * 2 * PI + this_angle);
// }

//开环
float velocityOpenLoop(float target_velocity, float Uq)
{
    float Ts = 1E-3f;
    shaft_angle += Angle_Constrain(target_velocity * Ts);
    Set_Phase_Voltage(Uq, 0, shaft_angle);
    // DMA_Printf("%f\r\n",Electric_Angle(shaft_angle));
    return Uq;
}

//位置闭环
void Set_Velocity(float Target)
{
    float Sensor_Angle = sum_angle;
    // float Uq = simple_pid(Target - DIR * Sensor_Angle, 0.7, 0, 0.0f);
    // Set_Phase_Voltage(Uq, 0, Electric_Angle(Sensor_Angle));
    float Kp = 0.133f;
    Set_Phase_Voltage(Constrain(Kp * (Target - DIR * Sensor_Angle)*180/PI, -6, 6), 0, Get_Electric_Angle());
    // DMA_Printf("%f\r\n", Constrain(Kp * (Target - DIR * Sensor_Angle), -6, 6));
}

//速度闭环
void Speed_Velocity(float Target_Speed)
{
    float err = -(Target_Speed - real_speed);
    float Uq = simple_pid(err, 0.28f, 0.2f, 0.0f);
    Uq = Constrain(Uq, -6, 6);
    Set_Phase_Voltage(Uq, 0, Get_Electric_Angle());
}

float simple_pid(float error,float kp, float ki, float kd)
{
    float P = kp * error;
    float I = Last_intergration + ki * (error + Last_Error) * 5E-3f * 0.5f;
    I = Constrain(I, -6, 6);
    float D = kd * (error - Last_Error) / 5E-3f;

    float output = P + I + D;
    output = Constrain(output, -6, 6);

    Last_Error = error;
    Last_intergration = I;

    return output;
}

float my_abs(float a)
{
    return (a >= 0) ? a : -a;
}